#include "pid.h"

//PID1 Position_pid;
//PID2 Add_pid;
void Position_pid_update(PID1* pid)
{
  pid->out = (pid->p * pid->err) +( pid->d * ( pid->err - pid->last_err ) );
  pid->last_err = pid->err;
}

void Add_pid_update(PID2* pid)
{
	float ep,ei,ed;
	ep = pid->err - pid->last_err;
	ei = pid->err;
	ed = pid->err - 2*pid->last_err + pid->prev_err;
  pid->pidout = pid->pGain * ep + pid->iGain*ei + pid->dGain*ed;
  pid->prev_err = pid->last_err;
	pid->last_err = pid->err;
}
